/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_NaviEnhance.c
 *
 *  Created on: 2022年7月17日
 *      Author: 64435
 */

#include "LOS_NaviEnhance.h"

//原始观测量轮询：2s一次

LOS_NaviEnhance_t STA_NaviEnhance;

uint8_t NaviEnhance_CMD_Status[3]={0x00,0x10,0x01};  //基本状态遥测轮询指令
uint8_t NaviEnhance_CMD_Result[3]={0x00,0x10,0x02};  //运算结果遥测轮询指令
//uint8_t pack[54]={0x0a, 0x6d, 0xd7, 0x82, 0xfe, 0x71, 0xdc, 0xce,0x02, 0xd4, 0x7b, 0xe0, 0x01, 0xff, 0xe1, 0xa5, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x20,0x00, 0x00, 0x00, 0x10, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa,0xaa,0xaa,0xaa,0xaa,0x91,0x00};
uint8_t navi_test_flag=0;
uint8_t navi_dabuf[370];


void LOS_NaviEnhance_CMD(uint8_t *cmd)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    
    SNP_CMD.SNP_dest = nID_NaviEnhance;    //导航增强模块
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x00;                //内容识别码：星务至导航增强模块轮询指令
    SNP_CMD.SNP_len = 11;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0X00), FP_CMD, 8);  
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0XFF), cmd, 3);
}

void LOS_NaviEnhance_UP()
{
    uint8_t upload[72]={0x03, 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x6E, 0x6A, 0x47, 0x00, 0x74, 0x6A, 0x47, 0x00, 0x00, 0x22, 0xC2, 0x33, 0x33, 0x0F, 0xC2, 0x00, 0x80, 0xAD, 0x43, 0x00, 0x00, 0xB6, 0x43, 0x33, 0x33, 0x93, 0xC1, 0x33, 0x33, 0x93, 0xC1, 0xCD, 0xCC, 0xCC, 0x3D, 0x0A, 0xD7, 0x23, 0x3C, 0x00, 0x00, 0xC0, 0x40, 0x9A, 0x99, 0x19, 0x3E, 0x00, 0x00, 0x20, 0x41, 0x00, 0x00, 0x00, 0x3F, 0x33, 0x33, 0x2E, 0x42, 0xCD, 0xCC, 0xEC, 0x40};
    LOS_CAN_SendPack(LOS_CAN_NUM1, nID_NaviEnhance, upload, 72);
}

void LOS_NaviEnhance_OriginData(uint8_t *odata)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint32_t CNT = 0;         //数据计数器
    uint8_t CNT_Frame = 0X01; //帧计数器

    SNP_CMD.SNP_dest = nID_NaviEnhance;    //导航增强模块
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x03;                //内容识别码：星务至导航增强模块原始观测量
    SNP_CMD.SNP_len = 509;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0X00), FP_CMD, 8);                //首帧发送
    while(CNT<496)
    {
        LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, CNT_Frame), &odata[CNT], 8);  //中间帧发送
        CNT += 8;
        CNT_Frame++;
    }
    //尾帧单独处理
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0xFF), &odata[CNT], 5);           //尾帧发送
}

// void LOS_NaviEnhance_Test()
// {
//     LOS_SNP_t SNP_CMD;        //首部配置结构体
//     uint8_t FP_CMD[8];        //首部内容
//     uint32_t CNT = 0;         //数据计数器
//     uint8_t CNT_Frame = 0X01; //帧计数器
//     uint8_t i=0;

//     SNP_CMD.SNP_dest = nID_NaviEnhance;    //导航增强模块
//     SNP_CMD.SNP_source = 0xF0;             //OBC发送
//     SNP_CMD.SNP_rec = 0x03;                //内容识别码：星务至导航增强模块原始观测量
//     SNP_CMD.SNP_len = 488;                 //长度（包含首部）
//     SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
//     LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
//     LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0X00), FP_CMD, 8);                //首帧发送
//     while(CNT<472)
//     {
//         LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, CNT_Frame), &pack2[CNT], 8);  //中间帧发送
//         CNT += 8;
//         CNT_Frame++;
//     }
//     //尾帧单独处理
//     LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0xFF), &pack2[CNT], 8);           //尾帧发送
// }


void LOS_NaviEnhance_Star( )
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint32_t CNT = 0;         //数据计数器
    uint8_t CNT_Frame = 0X01; //帧计数器
    uint8_t testdata[8]={0x66,0x66,0x66,0x66,0x66,0x66,0x66,0x55};

    SNP_CMD.SNP_dest = nID_NaviEnhance;    //导航增强模块
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0x02;                //内容识别码：星务至导航增强模块原始观测量
    SNP_CMD.SNP_len = 64;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0X00), FP_CMD, 8);                //首帧发送
    while(CNT<48)
    {
        LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, CNT_Frame), testdata, 8);  //中间帧发送
        CNT += 8;
        CNT_Frame++;
    }
    //尾帧单独处理
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_NaviEnhance, 0xFF), testdata, 8);           //尾帧发送
}


void LOS_CAN_NaviEnhanceCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;
    uint8_t sum_check = 0;
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t canmsgrec = 0xFF;       //消息类型（对应首部的rec）
    static uint32_t CNT_OriginData = 0;    //原始观测量接收计数器

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
        }
        else 
        {
           canerrflag = 1; 
        }

        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            switch(can_re->msg[6])
            {

            }
        }
    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            memcpy(&navi_dabuf[362], &can_re->msg[0],8);
        }
        
        canerrflag = 0;   //最后一帧时清除标志位
        canmsgrec = 0xFF; //同上
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (id_filter[3] == 0)
            {
                memcpy(&navi_dabuf[0], &LOS_Time_SecFromBoot, 4);
                memcpy(&navi_dabuf[4], &LOS_Time_MilSec, 2);
                memcpy(&navi_dabuf[6], &LOS_Time_Sec, 4);
            }
            memcpy(&navi_dabuf[10 + 8*(id_filter[3]-1)], &can_re->msg[0],8);
        }
    }
}


void LOS_NaviEnhance_DataTest()
{
    LOS_CAN_SendPack(LOS_CAN_NUM1, 0x66, navi_dabuf, 376);
}


void LOS_NaviEnhance_PWROn()
{
    LOS_EPS_Switch(4, SWITCH_ON);
    STA_NaviEnhance.CMD = 1;
}


void LOS_NaviEnhance_PWROff()
{
    LOS_EPS_Switch(4, SWITCH_OFF);
    STA_NaviEnhance.CMD = 0;
}
